﻿using System;
using blue.connection.mag_guide;

namespace main
{
    class Program
    {
        static void Main(string[] args)
        {
            //Test for Performance
            //TestMagneticGuideCon test = new TestMagneticGuideCon();
            //test.test("COM5",10000, 30);
            //test.showLog();

            //Test for Function
            MagneticGuide mag_ = new MagneticGuide("COM5");
            test_getDCenter(mag_);
        }

        private static void test_getDCenter(MagneticGuide mag_)
        {
            while (true)
            {
                if (mag_.updateSensorDataRT())
                {
                    byte[] data = mag_.getRawData();
                    double[] pos_f1 = mag_.getDoubleCenter(SensorID.FORWARD1);
                    double[] pos_f2 = mag_.getDoubleCenter(SensorID.FORWARD2);
                    double[] pos_b1 = mag_.getDoubleCenter(SensorID.BEHIND1);
                    double[] pos_b2 = mag_.getDoubleCenter(SensorID.BEHIND2);
                    double[] pos_l = mag_.getDoubleCenter(SensorID.LEFT);
                    double[] pos_r = mag_.getDoubleCenter(SensorID.RIGHT);

                    System.Console.WriteLine("(F1,F2,B1,B2,L,R) = {0}/{1} ,{2}/{3} ,{4}/{5} ,{6}/{7} ,{8}/{9} ,{10}/{11}",
                                            pos_f1[0], pos_f1[1], pos_f2[0], pos_f2[1], pos_b1[0], pos_b1[1], pos_b2[0], pos_b2[1]
                                            , pos_l[0], pos_l[1], pos_r[0], pos_r[1]);
                    //Console.WriteLine("Sensor 1&2 " + showByteAsBit(data));
                }
                else
                {
                    System.Console.WriteLine("failed to update data");
                    System.Threading.Thread.Sleep(500);
                }
            }
        }


        private static void test_getCenter(MagneticGuide mag_) {
            while (true)
            {
                if (mag_.updateSensorDataRT())
                {
                    byte[] data = mag_.getRawData();
                    double pos_f1 = mag_.getCenter(SensorID.FORWARD1);
                    double pos_f2 = mag_.getCenter(SensorID.FORWARD2);
                    double pos_b1 = mag_.getCenter(SensorID.BEHIND1);
                    double pos_b2 = mag_.getCenter(SensorID.BEHIND2);
                    double pos_l = mag_.getCenter(SensorID.LEFT);
                    double pos_r = mag_.getCenter(SensorID.RIGHT);

                    //System.Console.WriteLine("(F1,F2,B1,B2,L,R) = {0} ,{1} ,{2} ,{3} ,{4} ,{5}",
                    //                        pos_f1, pos_f2, pos_b1, pos_b2, pos_l, pos_r);
                    Console.WriteLine("Sensor 1&2 " + showByteAsBit(data));
                }
                else
                {
                    System.Console.WriteLine("failed to update data");
                    System.Threading.Thread.Sleep(500);
                }
            }
        }

        private static string showByteAsBit(byte[] data) {
            string result = "";
            // Sensor 1
            result += byteToString(data[10]);
            result += " ";
            result += byteToString(data[11]);
            result += "  ";
            // Sensor 2
            result += byteToString(data[12]);
            result += " ";
            result += byteToString(data[13]);

            return result;
        }

        private static string byteToString(byte data) {
            return Convert.ToString(data,2).PadLeft(8,'0');
        }

    }
}